#include <QCoreApplication>
#include "DeltaKinematics.h"
#include "DeltaInverseKinematicsM2.h"
#include "DeltaKinematicsM2.h"
#include <QDebug>

using namespace std;


typedef DeltaKinematics<double>::DeltaVector DV;

int main(int argc, char *argv[])
{
    QCoreApplication a(argc, argv);

    double sb=0.660;					  ///< base equilateral triangle side [ mm ]上基座等效三角形的边长　比如６６０
    double sp=0.90;					  ///< platform equilateral triangle side [ mm ]平台等效三角形的边长　比如９０
    double L=0.200;						  ///< upper legs length [ mm ]上退长　比如２００
    double l=0.530;

//    double sb=660;					  ///< base equilateral triangle side [ mm ]上基座等效三角形的边长　比如６６０
//    double sp=90;					  ///< platform equilateral triangle side [ mm ]平台等效三角形的边长　比如９０
//    double L=200;						  ///< upper legs length [ mm ]上退长　比如２００
//    double l=530;


    DeltaKinematics<double>::DeltaGeometricDim test_robot_dim = {sb,sp,L,l,0.070,-35,55};
    DeltaKinematics<double> test_robot(test_robot_dim);

    DeltaKinematics<double>::DeltaVector my_vector = {0,0,-0.500,0,0,0};
//        my_vector.Print();



    //1
    test_robot.CalculateIpk(&my_vector, 1);
        qDebug()<<"1CalculateIpk 坐标"<<my_vector.x<<my_vector.y<<my_vector.z
               <<"result 关节"<<my_vector.phi1<<my_vector.phi2<<my_vector.phi3;
    my_vector.z = 0.0;
    test_robot.CalculateFpk(&my_vector, 1);
    qDebug()<<"1CalculateFpk　关节"<<my_vector.phi1
           <<my_vector.phi2<<my_vector.phi3
          <<"result 坐标"<<my_vector.x<<my_vector.y<<my_vector.z;


    //2
    double angleB1;
    double angleB2;
    double angleB3;

    DeltaInverseKinematicsM2 IPK(&angleB1,&angleB2,&angleB3,L,l,sb,sp);
    IPK.set(my_vector.x,my_vector.y,my_vector.z);
    qDebug()<<"2CalculateIpk　坐标"<<my_vector.x<<my_vector.y<<my_vector.z
           <<"result 关节"<<angleB1/M_PI*180.0<<angleB2/M_PI*180.0<<angleB3/M_PI*180.0;

    double xResult;
    double yResult;
    double zResult;

    DeltaKinematicsM2 FPK(&xResult,&yResult,&zResult,L,l,sb,sp);
//    FPK.set(my_vector.phi1/180.0*M_PI,my_vector.phi2/180.0*M_PI,my_vector.phi3/180.0*M_PI);
    FPK.set(angleB1,angleB2,angleB3);
    qDebug()<<"2CalculateFpk　关节"<<angleB1/M_PI*180.0<<angleB2/M_PI*180.0<<angleB3/M_PI*180.0
              <<"result 坐标"<<xResult<<yResult<<zResult;

    return a.exec();
}
